/**
 * @file mercury_u2_camera.cpp
 * @author LauZanMo (LauZanMo@whu.edu.cn)
 * @brief
 * @version 1.0
 * @date 2021-09-22
 *
 * @copyright Copyright (c) 2021 i2Nav
 *
 */
#include <nodelet/loader.h>
#include <cstdlib>
#include <utility>

#include "mercury_u2_camera.hpp"

using namespace std;

namespace mercury_u2_camera {

void MercuryU2CameraNodelet::onInit() {
    m_nh = this->getMTPrivateNodeHandle();
    image_transport::ImageTransport it(m_nh);

    m_nh.getParam("device_id_list", m_device_id_list);

    m_nh.param<string>("frame_id", m_frame_id, "camera");
    m_nh.param<int>("max_auto_shutter_value", m_max_auto_shutter_value, 1000);
    m_nh.param<int>("min_auto_shutter_value", m_min_auto_shutter_value, 10);
    m_nh.param<int>("max_auto_gain_value", m_max_auto_gain_value, 64);
    m_nh.param<int>("min_auto_gain_value", m_min_auto_gain_value, 16);
    m_nh.param<double>("get_image_frequency", m_get_image_frequency, 60.0);
    m_nh.param<bool>("hardware_trigger", m_hardware_trigger, false);

    // 相机初始化
    assert(GXInitLib() == GX_STATUS_SUCCESS);

    // 枚举当前可用设备
#ifdef GET_DEVICE_BASE_INFO
    GXUpdateDeviceList(&m_device_num, 1000);
#else
    m_device_num = m_device_id_list.size();
#endif
    ROS_INFO_STREAM("Device number is: " << m_device_num);
    assert(m_device_num > 0);

#ifdef GET_DEVICE_BASE_INFO
    // 获取所有设备基础信息
    mp_device_base_info = new GX_DEVICE_BASE_INFO[m_device_num];
    size_t size = m_device_num * sizeof(GX_DEVICE_BASE_INFO);
    assert(GXGetAllDeviceBaseInfo(mp_device_base_info, &size) == GX_STATUS_SUCCESS);
#endif

    // 打开设备、设置属性并申请图像存储空间
    ROS_INFO_STREAM("Try to open device.." << endl);

    mp_cam_pub = new image_transport::Publisher[m_device_num];
    mp_device_handle = new GX_DEV_HANDLE[m_device_num];
    mp_ros_image = new sensor_msgs::Image[m_device_num];
    mp_frame_data = new GX_FRAME_DATA[m_device_num];

    for (uint32_t i = 0; i < m_device_num; i++) {
#ifdef GET_DEVICE_BASE_INFO
        ROS_WARN_STREAM("Current device SN: " << mp_device_base_info[i].szSN);
#else
        ROS_WARN_STREAM("Current device SN: " << m_device_id_list[i]);
#endif

        mp_cam_pub[i] = it.advertise("camera_" + to_string(i + 1), 1);
        OpenDevice(i);
        SetDeviceProperties(i);
        ApplyImageStorage(i);
        SetAcquireMode(i);

        ROS_INFO_STREAM("Current device is done." << endl);
    }
    ROS_INFO_STREAM("All device open.");

    // 启动采集图像定时器
    m_get_image_timer =
        m_nh.createTimer(ros::Duration(1.0 / m_get_image_frequency), &MercuryU2CameraNodelet::GetImageCb, this);
}

MercuryU2CameraNodelet::MercuryU2CameraNodelet() {}

MercuryU2CameraNodelet::~MercuryU2CameraNodelet() {
    // 停止采集并关闭设备
    for (uint32_t i = 0; i < m_device_num; i++) {
        GXSendCommand(mp_device_handle[i], GX_COMMAND_ACQUISITION_STOP);
        GXCloseDevice(mp_device_handle[i]);
    }
    GXCloseLib();

    // 释放动态内存
#ifdef GET_DEVICE_BASE_INFO
    delete[] mp_device_base_info;
#endif
    delete[] mp_device_handle;
    delete[] mp_cam_pub;
    delete[] mp_ros_image;
    for (uint32_t i = 0; i < m_device_num; i++)
        free(mp_frame_data[i].pImgBuf);
    delete[] mp_frame_data;
}

void MercuryU2CameraNodelet::OpenDevice(uint32_t id) {
    // 设置开启参数
    GX_OPEN_PARAM open_param;
    open_param.accessMode = GX_ACCESS_EXCLUSIVE;
    open_param.openMode = GX_OPEN_SN;
#ifdef GET_DEVICE_BASE_INFO
    open_param.pszContent = mp_device_base_info[id].szSN;
#else
    open_param.pszContent = const_cast<char *>(m_device_id_list[id].c_str());
#endif
    assert(GXOpenDevice(&open_param, &mp_device_handle[id]) == GX_STATUS_SUCCESS);
}

void MercuryU2CameraNodelet::SetDeviceProperties(uint32_t id) {
    int64_t value_min, value_max;
    GX_INT_RANGE range;

    // 自动白平衡
    GXSetEnum(mp_device_handle[id], GX_ENUM_BALANCE_WHITE_AUTO, GX_BALANCE_WHITE_AUTO_CONTINUOUS);

    // 自动黑电平
    GXSetEnum(mp_device_handle[id], GX_ENUM_BLACKLEVEL_AUTO, GX_BLACKLEVEL_AUTO_CONTINUOUS);

    // 自动曝光
    GXGetIntRange(mp_device_handle[id], GX_INT_AUTO_SHUTTER_VALUEMAX, &range);
    GXSetInt(mp_device_handle[id], GX_INT_AUTO_SHUTTER_VALUEMAX,
             m_max_auto_shutter_value < range.nMax ? m_max_auto_shutter_value : range.nMax);
    GXGetInt(mp_device_handle[id], GX_INT_AUTO_SHUTTER_VALUEMAX, &value_max);
    ROS_INFO_STREAM("Max auto shutter value range: " << range.nMin << " to " << range.nMax);

    GXGetIntRange(mp_device_handle[id], GX_INT_AUTO_SHUTTER_VALUEMIN, &range);
    GXSetInt(mp_device_handle[id], GX_INT_AUTO_SHUTTER_VALUEMIN,
             m_min_auto_shutter_value > range.nMin ? m_min_auto_shutter_value : range.nMin);
    GXGetInt(mp_device_handle[id], GX_INT_AUTO_SHUTTER_VALUEMIN, &value_min);
    ROS_INFO_STREAM("Min auto shutter value range: " << range.nMin << " to " << range.nMax);
    ROS_WARN_STREAM("Current auto shutter value range: " << value_min << " to " << value_max);

    GXSetEnum(mp_device_handle[id], GX_ENUM_EXPOSURE_AUTO, GX_EXPOSURE_AUTO_CONTINUOUS);

    // 自动增益
    GXGetIntRange(mp_device_handle[id], GX_INT_AUTO_GAIN_VALUEMAX, &range);
    GXSetInt(mp_device_handle[id], GX_INT_AUTO_GAIN_VALUEMAX, m_max_auto_gain_value);
    GXGetInt(mp_device_handle[id], GX_INT_AUTO_GAIN_VALUEMAX, &value_max);
    ROS_INFO_STREAM("Max auto gain value range: " << range.nMin << " to " << range.nMax);

    GXGetIntRange(mp_device_handle[id], GX_INT_AUTO_GAIN_VALUEMIN, &range);
    GXSetInt(mp_device_handle[id], GX_INT_AUTO_GAIN_VALUEMIN, m_min_auto_gain_value);
    GXGetInt(mp_device_handle[id], GX_INT_AUTO_GAIN_VALUEMIN, &value_min);
    ROS_INFO_STREAM("Min auto gain value range: " << range.nMin << " to " << range.nMax);
    ROS_WARN_STREAM("Current min auto gain value range: " << value_min << " to " << value_max);

    GXSetEnum(mp_device_handle[id], GX_ENUM_GAIN_AUTO, GX_GAIN_AUTO_CONTINUOUS);
}

void MercuryU2CameraNodelet::ApplyImageStorage(uint32_t id) {
    int64_t value;

    // 设置图像坐标系名
    mp_ros_image[id].header.frame_id = m_frame_id + "-" + to_string(id);

    // 设置图像编码
    GXGetEnum(mp_device_handle[id], GX_ENUM_PIXEL_FORMAT, &value);

    switch (value) {
    case GX_PIXEL_FORMAT_MONO8:
        mp_ros_image[id].encoding = "mono8";
        break;
    case GX_PIXEL_FORMAT_MONO16:
        mp_ros_image[id].encoding = "mono16";
        break;
    case GX_PIXEL_FORMAT_BAYER_GB8:
        mp_ros_image[id].encoding = "bgr8";
        break;
    case GX_PIXEL_FORMAT_BAYER_RG8:
        mp_ros_image[id].encoding = "rgb8";
        break;
    case GX_PIXEL_FORMAT_BAYER_BG8:
        mp_ros_image[id].encoding = "bgra8";
        break;

    default:
        static_assert(true, "Illegal format.");
        break;
    }

    // 设置图像分辨率
    GXGetInt(mp_device_handle[id], GX_INT_WIDTH, &value);
    mp_ros_image[id].width = value;
    GXGetInt(mp_device_handle[id], GX_INT_HEIGHT, &value);
    mp_ros_image[id].height = value;

    if (mp_ros_image[id].encoding == "mono8" || mp_ros_image[id].encoding == "mono16") {
        mp_ros_image[id].step = mp_ros_image[id].width;
    } else if (mp_ros_image[id].encoding == "bgr8" || mp_ros_image[id].encoding == "rgb8") {
        mp_ros_image[id].step = mp_ros_image[id].width * 3;
    } else if (mp_ros_image[id].encoding == "bgra8") {
        mp_ros_image[id].step = mp_ros_image[id].width * 4;
    } else
        static_assert(true, "Illegal format.");

    // 申请图像内存
    mp_ros_image[id].data.resize(mp_ros_image[id].height * mp_ros_image[id].step);
    GXGetInt(mp_device_handle[id], GX_INT_PAYLOAD_SIZE, &value);
    mp_frame_data[id].pImgBuf = malloc(value);

    ROS_WARN_STREAM("Current encoding: " << mp_ros_image[id].encoding);
    ROS_WARN_STREAM("Current resolution: " << mp_ros_image[id].width << "*" << mp_ros_image[id].height);
}

void MercuryU2CameraNodelet::SetAcquireMode(uint32_t id) {
    GXSetEnum(mp_device_handle[id], GX_ENUM_ACQUISITION_MODE, GX_ACQ_MODE_CONTINUOUS);
    GXSetEnum(mp_device_handle[id], GX_ENUM_TRIGGER_MODE, GX_TRIGGER_MODE_ON);
    GXSendCommand(mp_device_handle[id], GX_COMMAND_ACQUISITION_START);
}

void MercuryU2CameraNodelet::GetImageCb(const ros::TimerEvent &event) {
    for (uint32_t i = 0; i < m_device_num; i++) {
        if (!m_hardware_trigger) {
            // 发送软触发信号
            GXSendCommand(mp_device_handle[i], GX_COMMAND_TRIGGER_SOFTWARE);
        }

        // 获取并转换为ROS图像
        GXGetImage(mp_device_handle[i], &mp_frame_data[i], 1.0 / m_get_image_frequency);
        memcpy((char *)(&mp_ros_image[i].data[0]), mp_frame_data[i].pImgBuf,
               mp_ros_image[i].step * mp_ros_image[i].height);

        // 设置时间戳并发布图像
        mp_ros_image[i].header.stamp = event.current_real;
        mp_cam_pub[i].publish(mp_ros_image[i]);
    }
}

} // namespace mercury_u2_camera

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mercury_u2_camera::MercuryU2CameraNodelet, nodelet::Nodelet)